#include <algorithm/analytical_model/algorithm/cpu/VectorConvert.h>
#include <algorithm/analytical_model/algorithm/cpu/receiver/CylinderReceiver.h>
#include <algorithm/shared/utils/Vec3.h>

#include <vectorclass.h>

#include <cmath>

using namespace solar::cpu;
using namespace solar::cuda::inter_op;

auto solar::cpu::isInsideCylinder(const Float3& orig, const Float3& size, const Float3& pos) -> bool
{
    vcl::Vec2d vec_orig = {orig.x, orig.z};
    vcl::Vec2d vec_pos = {pos.x, pos.z};
    auto vec_l = vec_orig - vec_pos;
    return vcl::horizontal_add(vec_l * vec_l) <= size.x * size.x;
}

auto solar::cpu::getFocusCenter(const Float3& heliostat_position, const Float3& size,
                                const Float3& pos) -> Float3
{
    auto heliostat_position_vec = toVec3(heliostat_position);
    auto pos_vec = toVec3(pos);
    auto dir = (heliostat_position_vec - pos_vec).get_unit_vector();
    auto dir_data = dir.data();
    auto radius = size.x / std::hypot(dir_data[0], dir_data[2]);
    Vec3 tmp = vcl::mul_add(dir.get_vec(), vcl::Vec4d(radius), pos_vec.get_vec());
    auto tmp_data = tmp.data();
    return {static_cast<float>(tmp_data[0]), pos.y, static_cast<float>(tmp_data[2])};
}
